#include "hscb.h"

#include <string.h>
#include <thread>
#include <chrono>
#include <string>

#include "rtdev/rtdev.h"
#include "rtbus/rtbus.h"
#include "jkutil/configparser.h"
#include "jkutil/bitdataop.h"
#include "log/zlog.hh"
#include "hwdrv/hwdrv.h"

#define HSCB_ERR(fmt, ...) zlog()->error("[HSCB] " fmt, ##__VA_ARGS__)
#define HSCB_INFO(fmt, ...) zlog()->info("[HSCB] " fmt, ##__VA_ARGS__)

using rtdev::HScb;
using rtdev::HScbCab;
using rtdev::HScbRobotPower;
using rtdev::SafetyBoardItf;

static const uint32_t kControlWordIndex = 0x00200000;
static const uint32_t kControlWordDisplayIndex = 0x00200100;
static const uint32_t kStateWordIndex = 0x00200200;
static const uint32_t kUserDOIndex = 0x00630001;
static const uint32_t kUserDOFeedbackIndex = 0x00630301;
static const uint32_t kSafetyDIIndex = 0x00610002;
static const uint32_t kHscbVendorId = 0x661100;
static const uint32_t kHscbProductId = 0x00000001;
static const uint32_t kHscbRevisionNumber = 0x00000001;

HScbRobotPower::HScbRobotPower()
{
    feedbacks.current = 0;
    feedbacks.is_cmd_power_on = false;
    feedbacks.power = 0;
    feedbacks.voltage = 0;

    scb = nullptr;
}
// void HScbRobotPower::reset()
// {
//     for (auto& fb : feedbacks)
//     {
//         fb.is_cmd_power_on = false;
//         fb.voltage = 0;
//         fb.current = 0;
//         fb.power = 0;
//     }
// }
int HScbRobotPower::set_power(bool power, int robid)
{
    if (robid > 0)
        return -1;

    feedbacks.is_cmd_power_on = power;

    return 0;
}
double HScbRobotPower::get_power(int robid)
{
    if (robid > 0)
        return -1;
    if (scb->is_sim())
    {
        return 0;
    }
    return feedbacks.power;
}
double HScbRobotPower::get_voltage(int robid)
{
    if (robid > 0)
        return -1;
    if (scb->is_sim())
    {
        return 0;
    }
    return feedbacks.voltage;
}
double HScbRobotPower::get_current(int robid)
{
    if (robid > 0)
        return -1;
    if (scb->is_sim())
    {
        return 0;
    }
    return feedbacks.current;
}

HScb::HScb()
{
    bus_.reset();
    sim_flag_ = 0;
    cab_.scb = this;
    rob.scb = this;
}
const char* HScb::name() { return "HSCB"; }
int HScb::init(void* info, int sim)
{
    //根据配置信息找到总线
    auto bus = rtbus::BusMgr::get()->bus("scbbus");
    if (bus.get() == nullptr)
    {
        return -1;
    }
    bus_ = bus;

    sim_flag_ = sim;
    HSCB_INFO("HSCB init success.bus:{} sim:{}", bus_->get_name(), is_sim());
    return 0;
}

int HScb::boot()
{
    if (!is_sim())
    {
        //配置从站
        rtbus::RtCommuItf::SlaveInfo slave;
        slave.slaveid = 0;
        slave.cycletime_us = 1000;
        slave.shifttime_us = 500;
        slave.vendor_id = kHscbVendorId;
        slave.product_code = kHscbProductId;
        slave.revision_number = kHscbRevisionNumber;
        slave.enable_dc = 0;  //scb采用sm通讯，不需要dc
        bus_->config_slave(slave);

        auto slave2 = slave;  //hscb有两个从站
        slave2.slaveid = 1;
        bus_->config_slave(slave2);

        //配置pdo通道
        bus_->config_txpdo(kControlWordIndex, 4);         // control word
        bus_->config_txpdo(kUserDOIndex, 2);              // user DO
        bus_->config_rxpdo(kControlWordDisplayIndex, 4);  // Control Word Display
        bus_->config_rxpdo(kStateWordIndex, 4);           // State Word
        bus_->config_rxpdo(kSafetyDIIndex, 2);            // Safety DI
        bus_->config_rxpdo(kUserDOFeedbackIndex, 2);      // user DO feedback

        //启动总线
        if (bus_->active())
        {
            HSCB_ERR("bus {} active failed.", bus_->get_name());
            return -2;
        }

#if 0
        //读取硬件版本号
        const int retry_cnt = 100;
        uint16_t hw_type = 0, hw_ver = 0;
        int err = get_sdo_blocking(0x210201, &hw_type, 2, retry_cnt);
        if (err)
        {
            return err;
        }
        get_sdo_blocking(0x210202, &hw_ver, 2, retry_cnt);
        HSCB_INFO("hw type: {}, hw ver: {}", hw_type, hw_ver);

        //读取软件版本号
        uint32_t sw_ver = 0;
        get_sdo_blocking(0x210205, &sw_ver, 4, retry_cnt);
#else
        uint32_t statusword = 0;
        get_sdo_blocking(0x200200, &statusword, 4);
#endif
    }
    return 0;
}

int HScb::set_send_data(int busid)
{
    check_alive();

    if (!bus_)
        return -1;

    uint32_t controlword = 0;

    //此数据参看HSCB文档
    Util::set_bit((uint8_t*)&controlword, 0, cab_.cmd.dis_robot_commu);       // 请求伺服驱动刷机
    Util::set_bit((uint8_t*)&controlword, 1, rob.feedbacks.is_cmd_power_on);  // 机器人本体电源控制
    Util::set_bit((uint8_t*)&controlword, 2, cab_.cmd.pwr_ipc);  // 关闭IPC电源。1 表示控制器做好关机准备，允许SCB切断IPC电源并关机。
    Util::set_bit((uint8_t*)&controlword, 3, 0);
    Util::set_bit((uint8_t*)&controlword, 4, 0);
    Util::set_bit((uint8_t*)&controlword, 5, 0);
    Util::set_bit((uint8_t*)&controlword, 6, 0);
    Util::set_bit((uint8_t*)&controlword, 7, 0);
    bus_->set_txpdo_data(kControlWordIndex, (uint8_t*)&controlword);

    // memset(data, 0, 8);
    // bus_->set_txpdo_data(kUserDOIndex, data);

    return 0;
}
int HScb::get_recv_data(int busid)
{
    if (!bus_)
        return -1;
    uint32_t statusword = 0;
    auto alive_cnt = bus_->get_rxpdo_data(kStateWordIndex, (uint8_t*)&statusword);
    if (keep_alive(alive_cnt))
    {
        return -1;
    }

    //TODO: 解析statusword
    Util::get_bit((uint8_t*)&statusword, 0);  //急停
    Util::get_bit((uint8_t*)&statusword, 1);  //保护性停止
    Util::get_bit((uint8_t*)&statusword, 2);  //缩减模式
    Util::get_bit((uint8_t*)&statusword, 3);  //自动模式
    Util::get_bit((uint8_t*)&statusword, 4);  //手动模式
    Util::get_bit((uint8_t*)&statusword, 5);  //关机请求
    Util::get_bit((uint8_t*)&statusword, 6);  //是否应用固件
    Util::get_bit((uint8_t*)&statusword, 7);  //发生掉电

    uint16_t udo_feedback = 0;
    bus_->get_rxpdo_data(kUserDOFeedbackIndex, (uint8_t*)&udo_feedback);

    uint16_t safety_di = 0;
    bus_->get_rxpdo_data(kSafetyDIIndex, (uint8_t*)&safety_di);

    return 0;
}
int HScb::get_sdo_blocking(uint32_t objid, void* data, int s, int retry) { return bus_->get_sdo_blocking(0, objid, data, s, retry); }
int HScb::set_sdo_blocking(uint32_t objid, void* data, size_t s, void* readback, int retry) { return -1; }

HScbCab::HScbCab()
{
    cmd_cab_id = act_cab_id = 0;
    scb = nullptr;
}

int HScbCab::get_id() { return -1; }

int HScbCab::set_id(uint32_t id) { return -1; }
